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ABSTRACT 


This paper details a highly reliable computational algorithm for 
sequential least squares estir'ation (filtering) with process noise. The 
various modular components of the algorithm are described in detail so that 
their conversion to computer code is straightforward. These components 
can also be used to solve any least squares problem with possibly rank 
deficient coefficient matrices. 


KEYWORDS ; Kalman filters, square root filters, sequential least squares 
estimation, numerical solutions of linear least squares. 



ALGORITHM FOR SEQUENTIAL ESTIMATION 


Introduction 

In this paper we will describe a reliable iiomputational procedure for 
estimating the state vector of a noisy system from a set of noisy measure- 
ments. The state of the system, x(i) = [x^(i), X 2 (i), . . . ,x^(i)] , is 
described by a sequence of transition equations, 

x(k+l) = F(k)x(k) + G(k)o)(k) , k - 0,1,..., N-1 , (l) 

T 

where F(k) and G(k) are n x n matrices and U)(k) « [u)^(k), . . . ,ci)^(k)] 

is the process noise vector. The measurements z(k) = [z^(k), z„(k), 

.,,,z^(k)j" are given by, 

z(k) » H(k)x(k) + Q(k)v(k) (2) 

where H(k) and Q(k) are m x n and m x m matrices respectively and 

T 

v(k) = [v^(k), V 2 (k), . . . ,v^(k)] is the measurement noise vector. 

An estimation procedure for sequentially estimating the state x(k), 

(k » 0,1, . . . ,N-l), using orthonormal Householder transformations was described 
in a recent paper by Dyer and McReynolds (Ref. l). That paper showed that 
this procedure was equivalent to, and substantially more accurate than, the 
Kalman Filter (Ref. 2) and gave some numerical results. However, few details 
were given of the computational algorithm and there was little effort made 
to maximize the efficiency of the routine. In this paper details of a refined 
form of the algorithm are given. This new algorithm is substantially faster 
and requires only half the storage' of the algorithm indicated in Ref. 1. 

These points are described in Appendices A and B. 

It should be stated that this algorithm requires a rather large investment 
in programming to develop from the beginning. (We feel one-man year is a 
good estimate). We will, however, provide a set of (documented FORTRAN IV) 
subroutines to any interested requester. There is one feature of the pres’ent 
algorithm which we feel more than compensates for its complexity: it is 
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completely reliable in the sense that rank deficiencies will not cause a 
system to fail and roundoff errors are minimized. This program can, there- 
for.'t, be a welcome component of many automatic control systems. 

We will now review the algebraic operations required in the algorithm. 


Description of the Algorithm 

As stated above the problem is to estimate the state of a dynamic 
system in the presence of noise. It is assumed that the components of the 
noise vectors u) and v are statistically independent and gaussian with zero 
means and unit variances. This assumption is not restrictive because any 
set of correlated gaussian random variables maybe linearly transformed to 
a new set of independent gaussian random variables. One technique which 
effects this transformation is as follows: Let o)(k) denote correlated 

process noise with covariance C(k). Now employing the Cholesky square root 
algorithm (Ref, 3 ) s matrix D(k) is found such that C(k) = D(k)D(k) . By 
setting U) = D(k)w, equation (l) may be written 

x(k+l) = F(k) x(k) + G(k)D(k)w(k) 

where the components of w(k) are independent random parameters with zero 
mean and unit covaria '^e. 

The problem of estimating x(k) is equivalent to minimizing 
k 

j(k) =y^ {llv(i)l|^ + llw(i)||^} + l|x(l) - (3) 

with respect to the random sequences v(i) and w(i), (i » l,2,...,k), subject 
to the constraints of equations (l) and (2). In equation ( 3 ) x(l) denotes 
the a priori mean of x(l), while A(l) denotes the a priori covariSnce of 
x(l). 
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Let J , (k) denote the minimum return function 
opt' ' 

expressed in terms of x(k). Then 


1 


for this problem 


Here x(k) is the conditional mean of x(k), A(k) is the conditional covariance, 
2 

and r (k) denotes the sum of the squares of the residuals. 


The Dyer-McReynolds algorithm computes R(k) and d(k) where, 

E(k) = A‘2(k) 
d(k) « A"^(k) x(k) 


In terms of R(k) and d(k), the return ^Qp^(k) is given by 


= l|R(k)x(k) - d(k)ll^ + r^(k) 


Clearly, if R(k) is non-singular,. x(k) and A(k) are given by. 


x(k) R“^(k)d(k) 

and 

A(k) = R"^(k) R"^(k)^ 


( 6 ) 


il) 


The algorithm shall be developed in two steps. First, the measurements 
"til. 

at the k stage will be incorporated with the a priori information. 

'til S 

Secondly, the information will be transformed from the k to the k+1 
stage, corrupted by the effects of process noise. 

The best estimate of x(k) employing measurements z(l),,. ,.,z(k-l) is 
obtained by minimizing. 


k-1 k 

J(k) llv(i)|l2 + \\ Ilw(i)f + ]|x(l) - x(l)||2 

i=l i«l A (l) 

subject to the constraints imposed by Eqs. (l) and (2). 




1 See Cox, (Ref, 4 ) for the formulation of sequential es':imation in terms 
of dynamic programming. 



Note that, 


J(k) = J(k) + Ilv(k)||^ . 


step 1; Measurements 

Now assume that J(k) has been transformed to, 

J(k) = |l5(k)x(k) - d(k)|l^ + r^(k-l) 

(This will normally be the case. At the initial time R(1) and d(l) are 
formed from the a priori covariance and mean). The inclusion of measure 
ments implies the minimization of, 

J(k) = ||R(k)x(k) - d(k)||^ + ||v(k)|l^ + r^(k) 

Substituting for v(k) from equation (2) gives, 

J(k) = ||R(k)x(k) - l(k)l|^ + ||Q“^(k)H(k)x(k) - Q“^(k)z(k)f + r^(k) 


which may be written. 


j(k) = 


ii 


E(k) 


q,'^(k) H(k) 


x(k) - 


a(k) 


Q‘^(k) z(k) 


L 


+r^(k) 


Now an orthogonal (n+m) x (n+m) matrix, P, is constructed such that 


n 


n 


R(lO 


Q,"\k) H(k) 




)" 

}■ 


E(k) 


0 


}■ 


2 If the measurements are genuinely noisy then Q(k) is nonsingular. 
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Here R(k) is an upper triangular matrix. / Let d(k) and d’(k) be defined by. 


d(k) 

}n 

d(lc) } 



d’(k) )• 


( 14 ) 


The matrix P is a product of Householder trans formations, i.e,. 


P = 


P *P 
^n n-1 



where 

T 

Pi = + UiUi/pi , (i = l,...,n) , (X * m + n) . 

Each ?i is orthonormal and symmetric. It should be noted, however, that none 
of the full (n-Hn) x (n+ra) matrices 1 ?^ have to be formed explicitly. It is 
only necessary to store the X-vector Ui and the scalar ^i* Further details 
regarding the construction of these parameters are given in Appendix B, 
Algorithm 1. 


The return j(k) may now be written. 


J(k) = ||R(k)x(k) - d(k)|| + r^(k) 


(15) 


where r^(k) = r^(k) + ||d’(k)||^. The vectors d(k) and d'(k) are defined in Eq. (l4), 


The best estimate of x(k) and its covariance are given by. 


x(k) = i;“^(k)d(k) 
A(k) = B“^(k)R“^(k)^ 


(16) 


Details of the computation of x(k) and A(k) are given in Algorithms 3, 4, and 5 
of Appendix B, and the sequential processing of new data is outlined in Algorithm 2. 
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Step 2; Mapping and Process Noise 

Mapping forwards introduces process noise, and the return J(k+l) is 
given by, 

J(k+1) = l|w(k)||^ + |lR(k)x(k) - d(k)l|^ + r^(k) (l?) 

From equation (l), 

x(k) = F'"^(k) (x(k+l) - G(k)w(k)) 

Hence writing equation (17) in terms of x(k+l), 

J(k+1) = |lw(k)l|^ + l|R(k)F'^(k)x(k+l) - R(k)F'^(k)G(k)w(k) - d(k)|l^ (l8) 

This equation must now be minimized with respect to w(k) and w(k) eliminated. 
Equation (18) may be written. 


J(k+1) = 


n 


0 


R(k)F"^(k)0(k) R(k)F"-^(k) 


- 1 . 



■M mmm 


M- 



w(k) 


0 



x(k+l) 


d(k) 








+ r""(k) (19) 


The matrix of Eq, (I9) is the n x n identity matrix. 

The coefficient matrices R(k)F ^(k)G(k) and R(k)F ^(k) in Eq. (19) are 
computed in the following way. 


A product of n - 1 Householder ortho normal transformations 

T 

S = S , ... St, S. = (I + u.u./b.), (i = l,.,,,n-l), is found such that 
n-1 1’ 1 ' n 1 ^ ’ 

F(k) = ... 


( 20 ) 


where T is upper triangular. 

Since F(k) is nonsingular. 


F’^(k) = ... 


(21) 


Then premultiplying by R(k) /"nd postmultiplying by G(k) gives. 
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R(k)F'^(k)G(k) = R(k)(T'^(S^.j^...Sj^G(k))) (22) 

while, 

R(k)F‘V) = R(k)(...(T‘^Sjj_3),,.S3^) (23) 

In Algorithm 6 it will be shown that the formation of the matrix products on 
the left hand side of Eqs. (22) and (23) require only n additional storage 
locations. 


A (2n) X (2n) orthonormal matrix, again a product of 2n - 1 Householder 
transformations: 


X - . . . x^ 

is now chosen such that- 



(i = 1, ,,.,2n-l) , 


I 0 


A B. 

n 



R(k)F“^(k)G(k) , R(k)F”^(k) 


0 R(k+1) 


(24) 


In Algorithm 7 we will show that the right member of Eq, (24) can be generated 

2 

in such a way that only 2.5n + 3-5n+l memory locations are needed at each 

2 

step of the calculation. Exactly 2.5n of these cells are the working arrays 
which initially held the matrices F(k), G(k), and R(k), 


We further remark here that the matrix A in the right member of Eq. (24) 
is nonsingular. This follows from the observation that A is upper triangular 
and the modulus of each diagonal term has the value one at least. 


With, 


0 


3’(k+l) 

d(k) 


d(k+l) 


}• 

)» 


(25) 


the value of J(k+l) is 
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j(k+l) = ||R(k+l)x(k+l) - d(k+2)||''* + |lAw(k) + Bx(k+l) - d’(k+l)|j^ 

If R(k+l) is nonsingular the best estimate of x(k+l), given measurements 
'til 

through the k stage, is given by, 

x(k+l) = R"^(k+l)d(k+l) (26) 

The sn 'thed value of w(k) is given by, 

w(k) = A"^[d'(k+1) - Bx(k+l)] (2?) 

The covariance associated with x(k+l) is given by, 

A(k+1) = [S‘^(k+l)][E"^(k+l)]^ (28) 

The hypothesis that R(k+l) be nonsingular is not critical. We can replace 
the indicated inverse in Eq. (25) by a pseudoinverse (Ref. 3) which ■ 
always exists. 

In this case the covariance matrix of Eq, (28) no longer exists; one 
can, however agree to solve for certain of the variables and set the remaining 
ones to zero. This amoixnts to obtaining a pseudoinverse solution (in a 
limiting sense) with a weighted euclidean metric* In the latter case one 
can obtain a covariance matrix for the variables .which were solved for. 

The details of this are given in Algorithm 5 of Appendix B, 


APPENDIX A 

The flow diagram of AppendjvX A is intended to indicate the overall 
structure of the filter and how it makes use of the various component 
algorithms of Appendix B. These algorithms of Appendix B can be used for 
solving auy least squares problem. 
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APPENDIX B 


Many of the algorithms presented below have appeared in a slightly 
different form in Ref, 5 . They are repeated and expanded here for the sake 
of the completeness of this paper. Algorithm 3 is essentially due to 
Businger and Golub using a special case of Algorithm 1, 

Algorithm 1 

The basic Householder transformation; its construction and application, 
PURPOSE 

T 

Suppose that y = is an arbitrary vector of length m. Given 

three nonnegative integers t, and m. We wish to construct an orthonormal 
transformation Q = + uu /p such that for Qw: 

a) Components 1 through X are to be left unchanged 

b) Components X + 1 is permitted to change 

c) Components X + 2 through X + t + 1 are to be left unchanged 

d) Components X + t + 2 through m are to be zero. 

This can be accomplished with the following 
METHOD 

Let p = X + 1 and q - X + t + 2. 

With, 



Al.l 

1 


" typ + y^ + ... + y^] ‘(“SguCyp)) 

A1.2 

II 

1 

Q 

AI .3 

= a-Up (= -IHI^/2) 

AI.4 

“ y^^j (i “ 

A1.5 
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the matrix^ 

Q = + uu^/p AI.6 

is orthonormal and, 

/ y + (u y/p)u , ^ ^ 0 

Qy = < A1.7 

^ y , P = 0 

T 

“ [y2_> • • • cTj yj^+2’ * * * o,..,,o] ai .8 

which satisfies the requirements of a) through d) above. 


(in Eq, Al,2, sgn (y. ) = 1^ if y 2: 0, and equals -1 if y < 0. ) 

P P P 

The algorithm for computing the vector u and the scalar a is now given. 

The input to this algorithm will consist of three previously mentioned integers 

i, t, and m, the m- vector y and a single free cell to hold u upon output. 

P 

For later reference we will designate the output of this algorithm 

Hl(ji, t, m, u , y). The vector u will occupy just those positions of y which 
P 

were implicitly zeroed plus the one extra location labeled u . The scalar a 

P 

replaces y in storage. 


Type: Integer 

Real 

Double Pi'ecision 

Procedure: Hl(X, t, m, u , 

P 


i, t, m, i, p, q; 

y^> (i 
s; 


y) 


step Number 


D escription 


1 


2 


Set p :=^+l, q +t+2, 



2 

If i ^ m set s :-s+y^, i :=i+l and go 
to step 2. Else 

1 

Set a := [-sgn(y )]s2o 

ir 


3 
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Step Number 

4 

5 

REMARK 


Description 

Set u ;= w - a. 
P P 

Set y ;= a. 

P 


The vector u has now been calculated: the scalar 8 = au is later 

P 

available as the indicated product and need not be explicitly saved. 


The scalers u^ = y^, (i = q, require no change of (or extra) storage. 

T 

Assume now that c = [Ct,...,c ] is an m-vector, and that we wish to 
compute the matrix product Qc and place it into the storage previously occupied 
by c. 

T T 

From the equality cQ=(Qc) (Qis symmetric) we see that only matrix 
products of the form Qc need be discussed here. 

The matrix product Qc is given bj^ 

Qc ~ c + [(u^c)/p]u A1.9 

and so the matrix Q need not be explicitly formed. 

We will now present an algorithm for computing the matrix product indicated 
in AI. 9 . This procedure will be designated by the symbol H2(l, t, m, u, u^, c>. 


Type : Integer 

Real 

Double Precision 
Procedure; H2 (jC, t, m, u. 


X, t, m, i, p, q; 
c^, (i = l,...ym), 

s; 




(i — l^.a.^m)^ Oy pj 


Step Number 


Description 


Set p := £ + 1, q ;= £ + t + 2, 

s := u *c , i := q. 

P P 


1 
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Step Number 


C. 


3 

4 

5 

6 

7 

8 

9 

REMARK 


Description 

If i ^ m set s := s + u^*c^, i ;= i + 1, 
and go to step 2. Else 

If s = 0 go to step 9» Else 

Set 6 := a*u . 

P 

If p *= 0 go to step 9* Else 
Set s 1 = s/p. 

Set c :=c +u*s, i:=q. 

P P P ’ 

If i m set c. := c. + u.-s, i i + 1, 

111 

and go to step 8. Else 

The vector c has been replaced by Qc. 


Note that only those components numbered p = jt + 1, and 

(q = j& + tt2), are changed by premultiplication of c by Q. Further, if 

T 

these components of c are known to be zero (or, more generally u q = O) then 
Qc = c and no explicit computation is required. 
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Algorithm 2 
PURPOSE 

Sequential acceptance of equations to achieve upper triangular form as a 
preliminary step, 

METHOD 


Suppose we have a large linear least squares problem of the form. 

Ax - b A2,l 

The matrix A and the vector b Eire written in partitioned form, 



A2.2 


t-tVis-ms ofsrtVj + A ne m v ev'ii oofi'W K t e o + rkf* 1 orjcr+'.'h rn TVxa 

11 1 ^1 


integers m^ can be as small as one. 


Let ra = m.+,,,+m , We construct orthonormal matrices Q_,...,0 , each 
1 q ± q 

of which are a direct sum of an identity matrix and products of at most n 
Householder transformations and permutation matrices that 

n 1 




R 

0 

0 




A2.3 


Here R is upper triangular, d is an n-vector and |r| is the residual vector 
length if R is nonsingular. 



-l4- 


To this end set ^ = max(mj^, . . . and let W denote a compute working 
array with v n + 1 + rows and n + 1 columns. We will let W(i^:ig, 
denote the subarray of W consisting of rows i^ through i^ and columns 
through 0^. 

Type; Integer t, J^, r, 1, o, m, n; 

Real (i = l,,,,,q^)j sj 

Procedure: Sequential Triangularization 

Description 

Set t := 1 and Z - 0. (*) 

Set r := X + m, 
t 

Set W(X + l:r, l:n+l) := [A^,b^] 

Set i 1 
Compute 

Hl(i - 1, max(0,A-i), r, s, W(l;r,i:i)); 

If i ^ min(r,n), compute H2(i-l,max(0,X-i), 
r,W(l:r,i;i), s, W(l:r,j,j)), (j = i-i-1, . , . ,n-{-l). 
Set i := i+1, and go to step 6, Else 

If t ^ q, set t := t + 1, i := min[n+l,r], 
and go to step 2. Else 

The matrix A has been reduced to upper tri- 
angular form as indicated in A2.3. 

If there is an a priori matrix present in the first n rows of the 
working array W which is zero below the main diagonal, then one may 
start with JL = n. This a priori matrix will usually be the matrix A^ 
of Eq. A2.2. 


Step Number 
1 
2 

3 

4 

5 

6 

7 

8 
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remarks 


As we mentioned previously, one can have =1, (t = Then 

W need occupy at most (n+2)*(n+l) computer words* 

Following these transformations the strictly lower triangular part of 
W may contain remnants of the processing; these cells should be zeroed. The 
matrix R of A2.3 is in the upper triangular part of W; the vector d occupies 
W(l:n, n + l:n + l); the residual vector length (except possibly for sign) 
occupies W(n + l;n + 1, n + l:n + l). 



Algorithm 3 


PURPOSE 


Forward triangularization of square matrices with column scaling, 
column interchanges and rank determination. 

METHOD 

Suppose we wish to solve a n x n system (which may have a singular 
coefficient matrix) in the least squares sense. 

Ax = b A3 . 1 

Here A is an n x n real matrix of rank r ^ n and b is a real n- vector. 

We construct a nonsingular diagonal matrix D, a permutation matrix P and an 

T 

orthonormal matrix Q “ *** ” ^n ^ “ l,*..,n-l), 

such that 

T T -1 

A := Q TP D A3. 2 

so that if A is nonsingular, 

A“^ = DFf’^Q, 

Here, in general, T is upper triangular with its first r diagonal terms 
nonzero and with its last n-r rows identically zero. 

We remark here that the matrix in the right member of Eq. A3. 2 may 
actually be a replacement for A in the following sense: 

The data which constitutes the matrix A in the machine is usually only 

a representative member of a class of matrices (L which is determined by the 

original uncertainty in the data and the uncertainty caused by subsequent 

computer arithmetic operations on this data. Thus, it may be apparent during 

the calculation that there is a matrix k zQ, such that rank (a) < max [rank (A)]; 

keCl 

it is such a matrix A which replaces A in Eqs. A3.1 and A3. 2. 
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We now describe the details of this forward triangular ization procedure. 
Let W denote an n x (n+l) working array; W(i^:i2,d^:02)> before, will 
denote the subarray of W consisting of rows'i^ through i^ and columns 
through jg. 


Type : Integer 

Real 


^9 n, p(l:n), rank; 
t, c, d(l;n), u(l:n), eps; 


Procedure : Forward Triangularization 


Step Number 
1 


REMARK 


Description 
W := [A,b]; 

Scale the n columns of W. Save the reciprocals 
of these scale factors in d(j), (j * l,...,n). 


The optimal choice of scaling, when one is presented with data which is 
uncertain, is beyond the scope of this peper. One method which is simple 
to describe and has worked satisfactorily for us is to set the columns of W 
to have euclidean length one (unless they are identically zero). 


2 


*th 

Set u(j) := square of the length of the j 
column of W following the scaling of step 1, 
(j = l,...,n). 


3 Set 0 := 1, p(i) (i = l,...,n), and 

rank :*= n. 

4 If 1 < a < n set u(i) := u(i) - W(a-l:a-l>i:i)^j 

(i ~ a$***i>^)* ® ® 


5 If a = n go to step 13. Else 

6 • Find the smallest i ^ a> such that u(i) Ss u(i), 

U ^ o). 


7 


If i = a go to step 9. Else 



step Number 
8 


Description 
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Exchange columns i and j of W; Set u(i) u(j); 
Exchange p(j) and p(i). 

9 If u(o) ^ eps, set rank := min(rank, j - l); 

REMARK 

It may be that the rank of the matrix which is to replace the matrix in 
W is already known and need not be calculated as in step 8. This prior 
calculation of rank can be done quite effectively by computing a singular 
value decomposition fcv A. See Ref, 5 for further details. 

10 ' Compute 

Hl(o-l, 0, n, u(j), W(l:n, j:j)) 

11 Compute 

H2(j-1, 0, n, W(l:n, 5:d), u(j), W(l:n,i:i)), 

(i = j+1, . . , ,n+l). 

12 Set 0 := j:l and go to step 4. 

13 The algorithm indicated in A3. 2 is completed. 

In case the matrix A of Eq, A3.1 is nonsingular (or of rank n) we may compute 
the unique solution to this problem with the following steps; 

14 Solve the triangular system Ty = d for y; 

The matrix T is in the upper triangular part of 
the array V/; the vector d is in W(l;n, n+l;n+l). 

15 Apply the permutation matrix P to y. 


16 


Form the product x = D(Py) to obtain the 
unique solution. 
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REMARK 

The steps l4 through l6 described directly above can each replace the 
result of the previous one in storage. The details in steps l4 through l6 
above are not completely described here due to the fact that they constitute 
straightforward and extremely well-known coinputing methods. 
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Algorithm 4 
FU HPOSE 

Computing the solution of minimum length for rank deficient problems. 
METHOD 


The method described in Algorithm 3 allows us to assiime, with no loss 
of generality, that for a given system as in A3.1, we may write: 

T T -1 ' . 

A = Q TP D . A4.1 

T 

Here Q is a product of n - 1 Householder transformations, T is upper tri- 
angular with its first r diagonal terms nonzero and its last n - r rows 

T -1 

identically zero, P is a permutation matrix, and D is a diagonal matrix. 

Let us suppose, then, that W is again a working arm:-: as in Algorithm 3 
and that T is in the first r rows of the upper triangular part of W. 

We will first find r Householder transformations K ,...,K_ such that 

r’ ’ 1 


TK ... K = 
r 1 


S 0 
0 0 


A4.2 


where S is r x r upper triangular and nonsingular. 


The solution of minimum length or the pseudoinverse solution (Ref. 3) 


2 T — 2 

(with the norm ||x|| = x D x) is given by 

y = ... Qjb) 

c =: 1^^ r components of y , 


d = S“^c 


a4.3 

a4.4 

a4.5 
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6 ^ K • • • 

r 



}r 

}n-r 


and 


X = D(Pe) 

We now describe the computation of Eqs. a4.3 through A4.7. 


a4.6 


a4.7 


Type: Integer 

Real 


r, i, 0, n; 
t(l:n); 


Procedure; Backward Triangularization 


Step Number 


Description 


1 


Use Algorithm 3 to compute y of Eq. A4.3* 
Place y in W(l:n, n+l;n+l). 


2 


Set j ;® r. 


3 If j > 0, compute 

Hl(d-1, r-j, n, t(j), W(j;j, l:n)) and compute 
H2(j-1, r-j, n, W(o:j, l:n), t(j), 

W(i;i, l;n)), (i = j-l,...,!), (in this order). 

Then set j j+1 and go to step 3« Else 

4 Multiply the first r components of the vector 

in W(l:n, n+1: n+l) by s"^. Here S is the r x r 
upper triangular matrix in the first r rows of 

the upper triangular part of W. This multiplica- 
tion should be accomplished by solving Sd * c 
of Eq. A4.5. 

5 Then compute 

H2(i-1, r-i, n, W(i;i, lin), t(i), W(l;n, n+l; n+l)), 
(i - l,...,r), (in this order). 
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Step Number Description 

6 Set W(i:i, n+1; n+l) := 0, (i = r+l,...,n). 

7 Apply the permutation matrix P to the vector 

y in W(l;n, n+l: n+l). 

8 ^ Form the matrix product x = D(Py) in 

W(l:n, n+l: n+l). 

9 The pseudoinverse solution of Ax = b (with 

II 2 T -2 

respect to the norm ||x|| = x D x) is now 

in W(l:n, n+l:n+l). 

Often the pseudoinverse solution, whose calculation is defined above, 
must be replaced by the approximate solution obtained by netting the last 
n - r components of the vector x to zero. 


Thus 


X 


D(P 



a4.8 


where 



“ ^11 ^1 


a4.9 


Here T^, is the r x r upper triangular matrix formed with the first r columns 

.•••Is 

of the matrix T of Eq. A4.1, while y^ is the first r components of the 
vector y of Eq. A4.3* 


We will comment further on this in Algorithm 5« 
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Algorithm 3 
PURPOSE 


Computation of the covariance matrix. 


METHOD 


Let us suppose, as in Algorithm 4, that we have 

T T -1 
A Q-^TP D 


A5.1 


Let 


n-r 


T = 


^11 ^12 


0 


0 




A5.2 


where is r x r, upper triangular and nonsingular. In case either 
r = rank (A) = rank (t) = n, or the solution is obtained by setting the last 
n ■* r components of F D to zero, the (unsealed) covariance matrix of those 
variables which were solved for can be defined by 


P^D A5.3 

If r rank (A) = n, then 

C(A) = (A^A)”^ A5.4 

as can easily be verified. (See Ref. 7 .) 

We will now describe the algorithm for computing the right side of 
Eq. A5.3« The matrix T will be in the first r rows of the upper triangular 
part of the working array W. 


C(A) = DP 


m” m— 1 \ 

"ll'^ll^ 


T 


u 


0 


0 



Type; Integer r, i, Oj n, k, X, p; 

Real W; 

Double Precision s; 

Procedure; Covariance matrix computation 

Step Number Description 


1 

2 

3 

4 

5 

6 

7 

8 
9 

10 

11 

REMARK 


W(j;d, j:j) := l/w(j:j, (d = l,...,r) 

If r = 1 go to step 17 . Else 
Set d ;= 2, 

Set k ;= r + 2 - j. 

Set i ;- 2. 

Set p ;s=k + l- i, s ;=0, 

Set s ;= s + W(p;p, X;X)*W(X;X, k;k), 

(x “p lj*»«jk)« 

Set W(p;p, k;k) ;= -s*W(p;p, p;p). 

If i < k, set i ;= i + 1 and go to step 6. 
If 0 < r, set j ;= j + 1 and go to step 4. 
Set X := 1. 


The matrix T~^ has now replaced the matrix T^^ in the storage array 

12 Set i ;= X. 

13 Set s ;= 0. 
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Else 

Else 


l4 


Set s ;= s + W(X;X, d:d)*'W(i:i, d:d)> (d = i,..*,r). 



- 25 - 


Step Number 

15 

16 

17 


Description 

Set W(i:X, i:i) := s. 

Ifi<r, i :=i + l and go to step 13. “Slse 
If < r, set i := + 1 and go to step 12. Else 


REMARK 


-1 -1 ^ 

The upper triangular part of the symmetric matrix has now 


,-l 


replaced in storage 


18 Zero the last n-r columns of the upper tri 

angular part of W. 

T 

19 Compute W := PWP . 


20 Compute W := DWD, 

21 The upper triangular part of the symmetric 

matrix C(A) of Eq. A5.3 is now in the upper 
triangular part of the array W. 

REMARK 

In steps 19 and 20 only the upper triangular part of W need be referenced. 
We will not comment on these details. 
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Algorit hm 6 


PURPOSE 

Computation of the matrix products associated with forward mapping of 
process noise. 


METHOD 

In Eqs. (22) and (23) we see that matrix products of the forms RF 
and RF"^ must be formed where R is upper triangular, F is nonsingular 
and G is arbitrary. Ail of these matrices are n x n. 

Analogous with Eq. (22) set 

* 

F = T ' > (O-^ ” Ijj ’ i=l,...,n-l) • A6,1 

Then 

RF"^G = R(t”^Q^_^...Q^G) A6.2 

and 

RF”^ * r(t"*q^_^...q^) A6.3 

For the purpose of describing the formation of these matrix products, 
suppose that R is located in the upper triangular part of a working array W 
and that F and G are in partitioned form in an n x 2n working array Y. 


Type: 


Integer 

Real 




step Number 
1 


u(l:n), t(l:n); 

Description 
Set 0 I- !• 

If j < n, compute Hl(j-1, 0, n, u(j), Y(l:n, J:j)) 
and next compute H2(j“l, 0, n, Y(l:n, j:j), u(j), 
Y(l:n, i;i)), (i = j+l,...,n ); then set 
,j ;= j + 1 and go to step 2. Else 
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REMARK 

The matrix T is in the upper triangular part of the left half of Y; 
G is in the right half of Y, 


Step Number 
3 


4 

5 


Description 

Compute F by solving the n systems 
of n equations FX = G for X; the 
matrix X can replace G in storage in 
the right half of Y, 

Set t(j): =Y(j,j), (o=l,,.,,n). 

Compute the matrix T this matrix 
can replace T in storage in the upper 
triangular part of the first n columns 
of Y. (See Algorithm 5, Steps I-IO). 


REMARK 


The matrix F ^G is now in the right half of Y. 


6 Set j := n - 1, 

If 0 > 0 first set t(i) W(i;i, j:j) 
and then W(i;i, := 0, (i=j+l, . . . ,n). 

Next compute H2(j-1, 0, n, t(l:n), u(j), 
Y(i;i, l:n)), (i = l,..,,n). Else 

REMARK 

In step 6 the last n - j + 1 columns of the left half of Y are all that 
is affected by multiplication from the right by Q . 

tJ 


7 


8 


The working array Y contains the 
augmented matrix [F F ^G], Note 
that the order of these matrices is 
reversed from that required in 
Algorithm 7. 

-1 -1 

Compute the product R[F j F G], 

r -1 -In 

This matrix can replace IF , F GJ 
in the Y array. 
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Algorithm 7 
PURPOSE 


Forward triangularization when mapping forwards with process noise. 


METHOD 


As indicated in Eq. (30)j we wish to find an orthonormal matrix X such 


that for given n 

X n .matrices C., 

(i 

II 

2), 

and 

a given 







— M 





I 

n 

, 0 , 

0 


A 

, B 


XS 

= X 


> ^2 ^ 

d 


0 

, R 

’ ^2_ 


A7.1 


where both matrices A and R are upper triangular. The vector d^ is of length 
n as are the vectors e^, (i = 1, 2). The matrix B will, in general, have no 
special structure. The definition of the matrix S is self-explanatory. 


If the n X 2n matrix [Cj_ , C^] occupies part of 8.n (n+l)(2n+l) working 
array Y, and if an n x n working array W is available, then the right hand 
side of Eq. A?.l can be computed and stored in the working array Y together 

with the upper triangular part of the array W. In total this requires 

2 2 
2.5n + + 1 computer words; this is in marked contrast to the 4n + 2n 

cells of memory which might at first seem to be required to calculate the 

right side of Eq. A7.1. 


Let [c^,...,c_ ] denote the 2n column vectors of the n x 2n matrix 
[Ci , Cg]. The first column of the matrix which is the right factor of the 
middle term of Eq. A7.1 is the 2n vector 

T 

w^ = [1, 0,,..,0, c^] A7.2 

After constructing the Householder transformation 

^1 " ^2n ^l^l/^l 

such that 



= ± [1 + lloj^f [ 1 , 0,...,of 


A7.3 


The details of Algorithm 1 show that: 

n 

fn fp fp 

(1) After the matrix products X^[e^,Cj^]j (i=2,...,n), Xj^[0,c^], 

(i«n+l, . ^ ,2n), and3y]o,d^] are computed, only the first component 
or the last n components are possibly nonzero. The vectors e^ are 
the unit coordinate vector®. 

(2) Thus only one row of the matrices A and B and one component of the 

vector e. will be calculated at each step in the construction of 

a matrix X = X • . . X^ such that 
n 1 


XS = 


0 R 


A7.^^ 


The matrix of Eq. A7.4 is n x n but is not necessarily, upper 
triangular. 

(3) As the rows of A and B and the components of are calculated 


^cui j. 


■5 -r^rj^+ci 4-Vj^ rr o*»»Y^cnre V" pnd W 

J.lJlUVi/ UO wo. \Jki\tf ?vwa»A>.Xii^ mo. jk w a. n 


space has come available. 

We now present a step-by-step procedure which effects these space-and 
labor saving remarks. 


Type: 


Integer 

Real 


i, n; 


Step Number 


Description 


Move the 2n + 1 components of the n + 1 row 

s *t ijh 

of S (now in the 1 row of Y, say) to the 2n 

row of the working array Y. 


2 


Set 0 := 1 



- 30 - 


Step Number 


Description 


3 Set Y(1:1, i;i) := 0, (i = and 

Y(1:1, J;a) 1. 

4 If 0 n, compute Hl(0, 0, n+1, t, Y(l:n+1, 

and next compute 

H2(0, 0, n+1, Y(l:n+1, t, Y(l:n+1, i:i)), 

(i = j+1, . . . ,2n+l), 

Y(l:l, j:j) ;= Y(1:1, 2n+l:2n+l), Y(2:i, o:j) := 
Y(1:1, i:i), (i = n+l,...,2n), 

W(j:o, i:i) := Y(l:l, i:i), (i = J+l,...,n), 
u(j) 1= Y(1:1, o:j); then set j := J + 1 and 
go to step 3. Else 


REMARK 

T / T 

At this point B occupies^ Y( 2 : n+1, l:n); note that each column of B 

moves in to occupy the storage implicitly zeroed with the successive House- 

T 

holder transformations; the strictly lower triangular part of the matrix A 

T 

is in the strictly lower part of the array W; diagonal terms of A are now 
in u(l:n), 

5 Triangularize the matrix R now in Y(2:n+1, n+l:2n) 

with Algorithm 3* 

6 Place the strictly lower part of A into the 

lower part of Y(2:n+1, n+1: 2n). 

REMARK 


Step 6 completes the forward mapping procedure; a solution and its 
covariance may be obtained by means of Algorithms 3-5. 


. 
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The smoothed value of the process noise is then trivially computed by 

T 

means of Eq. (27). Recall that B is in Y(2:n+1, l:n), the strictly lower 
T 

part of A is in the strictly lower part of Y(2:n+1, n+1; 2n), the diagonal 
entries of A are in u(l:n), and the vector d*(k+l) of Eq. (27) is in 
Y(l:l, l:n). 

To restart the basic cycle the upper triangular matrix R together with 
the vector e^ of Eq, A7.1 are now in the upper part of Y(2:n+1, n+l:2n+l) and 
must be copied to the upper triangular part of W(l:n, i:n+l). 
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APPENDIX A 



PROCESS NEW 
MEASUREMENTS 
SEQUENTIALLY 
W a)f Alg 2 b) 


PROCESS NEW 
MEASUREMENTS 
(STEP 1) 


NO / WANT A SOLUTION \ YES 
\OR COVARIANCE MATRIX?/ 


GET F AND G 
MATRICES FOR FORWARD 
MAPPING 


COMPUTE RF G C, 


MAP WITH 
PROCESS NOISE 
(STEP 2) 


MAP FORWARD, AND 
COMPUTE SMOOTHED 
PROCESS NOISE 
Y; AIgs 6,7 


' WANT A SOLUTION \ YES 
VOR COVARIANCE MATRIX?/ 


a) W AND Y REFER TO 
WORKING AREAS IN THE 
COMPUTER OF DIMENSIONS 
(n + 1 +/x) X (n + 1) AND 
(n + 1) X (2n + 1) 
RESPECTIVELY. (HERE 
/!= MAX NUMBER OF NEW 
MEASUREMENTS PROCESSED 


b) Alg N DENOTES THE 
ALOGORITHM IN APPENDIX 
B WHICH DESCRIBES THE 
RELEVANT COMPUTATION 


AT OKIP TliXAP ^ 







APPENDIX A (contd) 









